from fairino import Robot
import time



class Robotmove:
    def __init__(self, ip_address='192.168.58.2'):
        self.position_dict={"4":[11.331,-78.245,108.114,-22.159,170.936,162.872],
                       "1":[7.914,-71.742,105.764,-16.765,171.424,162.874],
                       "3":[-0.278,-74.616,106.418,-10.217,160.066,162.873],
                       "5":[4.303,-81.694,110.953,-8.805,159.192,162.872],
                       "6":[4.275,-95.327,3.751,-8.896,159.276,162.871],
                       "2":[ 7.914,-71.742,105.764,-16.765,171.424,162.874]
                       }
        try:
            self.robot = Robot.RPC(ip_address)
            self.is_connected = True
        except Exception as e:
            print(f"无法连接到机械臂：{e}")
            self.is_connected = False

        self.tool = 0  
        self.user = 0  
        print(self.robot.GetRobotMotionDone())#判断机械臂是否运动完成
    def get_teach_point(self,name):
        """
        获取指定 teach point 的位置信息
        :param name: teach point 的名称
        :return: teach point 的位置信息，格式为 [x, y, z, rx, ry, rz]
        """
        ret = self.robot.GetRobotTeachingPoint(name)
        print("查询机器人示教管理点位数据错误码", ret)
        
    def set_crash_level(self,block=0):
        error = self.robot.SetCollisionStrategy(strategy=block)
        print("设置碰撞后策略错误码:",error)


    def move_joints(self, joint_id, velocity=100):
        """
        运动成功返回True
        运动失败返回False
        """
        try:
            # 检查 joint_id 是否在 position_dict 中
            if joint_id in self.position_dict:
                # 执行机器人移动
                error = self.robot.MoveJ(self.position_dict[joint_id], self.tool, self.user, vel=velocity,blendT=-1)#blend控制程序是否阻塞执行
                
                ing=False
                
                # print(self.robot.GetRobotMotionDone())
                # 等待运动完成
                while not ing:
                    time.sleep(0.1)
                    _,ret=self.robot.GetRobotMotionDone()
                    
                    if(ret==1):
                        ing=True
                    # print("运动中...",ret)
                
                if(error!=0):
                    return False
                else :
                    return True
               
            else:
                print(f"错误：找不到关节位置 ID {joint_id}")
                return False
                
        except Exception as e:
            print(f"运动过程发生错误: {str(e)}")
            
            
            print("关节空间运动指令错误码", error)
            return False

# 示例用法
if __name__ == '__main__':
    robot_arm = Robotmove()
    
